//
// Created by Hoyin on 2024/4/19.
//

#include "ObjectMatcher.h"

double ObjectMatcher::_calcDifference(const cv::Mat &src1, const cv::Mat &src2) {
	std::vector<cv::Mat> bgr_planes1, bgr_planes2;
	cv::Mat img1, img2;

	if (src1.cols * src1.rows <= src2.cols * src2.rows) {
		cv::resize(src1, img1, cv::Size(src1.cols, src1.rows));
		cv::resize(src2, img2, cv::Size(src1.cols, src1.rows));
	} else {
		cv::resize(src1, img1, cv::Size(src2.cols, src2.rows));
		cv::resize(src2, img2, cv::Size(src2.cols, src2.rows));
	}


	cv::GaussianBlur(img1, img1, cv::Size(5, 5), 0, 0);
	cv::GaussianBlur(img2, img2, cv::Size(5, 5), 0, 0);

	cv::split(img1, bgr_planes1);
	cv::split(img2, bgr_planes2);

	std::vector<cv::Mat> bgr_diff;
	for (int i = 0; i < src1.channels(); i++) {
		cv::Mat _diff;
		cv::absdiff(bgr_planes1.at(i), bgr_planes2.at(i), _diff);
		bgr_diff.push_back(_diff);
	}

	double diff;
	for (auto bgr_diff_i : bgr_diff) {
		for (auto iter = bgr_diff_i.begin<uchar>(); iter < bgr_diff_i.end<uchar>(); iter++) {
			diff += *iter.ptr;
		}
	}

//		utl::showImage("img1", img1);
//		utl::showImage("img2", img2);
//		utl::showImages("bgr_diff", bgr_diff);
//		cv::destroyAllWindows();
	return diff / (img1.rows * img1.cols);
}

void ObjectMatcher::detect(cv::Mat &src, std::vector<YOLOBox> &yoloResult) {
	if (!_yoloOK) {
		std::cerr << "YOLO not ok." << std::endl;
		return;
	}
	if (!_yolo.Detect(src, _net, yoloResult)) {
		std::cerr << "YOLO detect object failed." << std::endl;
		return;
	}
}

void ObjectMatcher::match(cv::Mat &queryField, cv::Mat &endField, std::vector<YOLOBox> &queryBoxes,
                          std::vector<YOLOBox> &endBoxes, std::vector<std::pair<int, int>> &matchMap) {
	cv::Mat endImage = endField.clone();
	cv::Mat queryImage = queryField.clone();
	matchMap.clear();

	for (int i = 0; i < queryBoxes.size(); i++) {
		std::vector<std::pair<int, double>> pairs;
		for (int j = 0; j < endBoxes.size(); j++) {
			double intensityDiff = _calcDifference(queryField(queryBoxes.at(i).box), endField(endBoxes.at(j).box));
			double positionDiff = norm(queryBoxes.at(i).box.tl() - endBoxes.at(j).box.tl());
//				std::cout << "Query " << i << " to " << "End " << j << ": " << intensityDiff << ", " << positionDiff << std::endl;
			pairs.emplace_back(j, intensityDiff + positionDiff);
		}
		double minDist = pairs.at(0).second;
		int minDistIdx = 0;
		for (int pairIdx = 0; pairIdx < pairs.size(); pairIdx++) {
			auto pair = pairs.at(pairIdx);
			if (pair.second < minDist) {
				minDist = pair.second;
				minDistIdx = pairIdx;
			}
		}
		matchMap.emplace_back( i, minDistIdx );
//		std::cout << "Query " << i << " ~ End " << minDistIdx << std::endl << " Score: " << minDist << std::endl;
	}
#if defined(ENABLE_IMG_LOG) && ENABLE_IMG_LOG == true
	std::default_random_engine randE(0);
		std::uniform_int_distribution<int> dist(0, 255);
		for (int i = 0; i < queryBoxes.size(); i++) {
			cv::Scalar clr(dist(randE), dist(randE), dist(randE));
			cv::rectangle(queryImage, queryBoxes.at(i).box, clr, 2);
			cv::putText(queryImage, std::to_string(i), queryBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
		}
		utl::showImage("Query Image", queryImage);
		for (int i = 0; i < endBoxes.size(); i++) {
			cv::Scalar clr(dist(randE), dist(randE), dist(randE));
			cv::rectangle(endImage, endBoxes.at(i).box, clr, 2);
			cv::putText(endImage, std::to_string(i), endBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
		}
		utl::showImage("End Image", endImage);
#endif
}

void ObjectMatcher::detectAndMatch(cv::Mat &image1, cv::Mat &image2, std::vector<YOLOBox> &yoloBoxes1,
                                   std::vector<YOLOBox> &yoloBoxes2, std::vector<std::pair<int, int>> &matchMap) {
	this->detect(image1, yoloBoxes1);
	this->detect(image2, yoloBoxes2);
	this->match(image1, image2, yoloBoxes1, yoloBoxes2, matchMap);
}

#if defined(BLOCK_DEPRECATED) && BLOCK_DEPRECATED == false

void ObjectMatcher::match_deprecated(cv::Mat &endField, cv::Mat &queryField, std::vector<YOLOBox> &endBoxes,
                                     std::vector<YOLOBox> &queryBoxes) {
	std::vector<double> convCore( { 1, 1, 1, 1 } );
	std::vector<Eigen::VectorXi> endFeatureVectors, queryFeatureVectors;

	_calcFeatureVectors(endField, endBoxes, convCore, 2, endFeatureVectors);
	_calcFeatureVectors(queryField, queryBoxes, convCore, 2, queryFeatureVectors);

	cv::Mat endImage = endField.clone();
	cv::Mat queryImage = queryField.clone();

	std::default_random_engine randE(0);
	std::uniform_int_distribution<int> dist(0, 255);

	for (int i = 0; i < queryFeatureVectors.size(); i++) {
		std::vector<std::pair<int, long>> pairs;
		for (int j = 0; j < endFeatureVectors.size(); j++) {
			Eigen::VectorXi diffVector = queryFeatureVectors.at(i) - endFeatureVectors.at(j);
//				Eigen::IOFormat iof(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[ ", " ]");
//				std::cout << diffVector.format(iof) << std::endl;
			std::pair<int, long> pair({ j, diffVector.norm() });
			pairs.push_back(pair);
		}
		double minDist = pairs.at(0).second;
		int minDistIdx = 0;
		for (int pairIdx = 0; pairIdx < pairs.size(); pairIdx++) {
			auto pair = pairs.at(pairIdx);
			if (pair.second < minDist) {
				minDist = pair.second;
				minDistIdx = pairIdx;
			}
		}
		std::cout << "Query " << i << " ~ End " << minDistIdx << std::endl << " Score: " << minDist << std::endl;
	}

	for (int i = 0; i < queryBoxes.size(); i++) {
		cv::Scalar clr(dist(randE), dist(randE), dist(randE));
		cv::rectangle(queryImage, queryBoxes.at(i).box, clr, 2);
		cv::putText(queryImage, std::to_string(i), queryBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
	}
	utl::showImage("Query Image", queryImage);
	for (int i = 0; i < endBoxes.size(); i++) {
		cv::Scalar clr(dist(randE), dist(randE), dist(randE));
		cv::rectangle(endImage, endBoxes.at(i).box, clr, 2);
		cv::putText(endImage, std::to_string(i), endBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
	}
	utl::showImage("End Image", endImage);
}

void ObjectMatcher::match_defaultHist(cv::Mat &endField, cv::Mat &queryField, std::vector<YOLOBox> &endBoxes,
                                      std::vector<YOLOBox> &queryBoxes) {
	cv::Mat endImage = endField.clone();
	cv::Mat queryImage = queryField.clone();

	std::default_random_engine randE(0);
	std::uniform_int_distribution<int> dist(0, 255);

	for (int i = 0; i < endBoxes.size(); i++) {
		std::vector<std::pair<int, double>> pairs;
		cv::Mat srcHist;
		_calcHistogram(endField(endBoxes.at(i).box), srcHist);
		for (int j = 0; j < queryBoxes.size(); j++) {
			cv::Mat queryHist;
			_calcHistogram(queryField(queryBoxes.at(j).box), queryHist);
			std::pair<int, double> pair({ j, cv::compareHist(srcHist, queryHist, cv::HISTCMP_CORREL) });
			pairs.push_back(pair);
		}
		double maxCorr = 0;
		int maxCorrIdx = 0;
		for (int pairIdx = 0; pairIdx < pairs.size(); pairIdx++) {
			auto pair = pairs.at(pairIdx);
			if (pair.second > maxCorr) {
				maxCorr = pair.second;
				maxCorrIdx = pairIdx;
			}
		}
		std::cout << "Query " << i << " ~ End " << maxCorrIdx << std::endl << " Score: " << maxCorr << std::endl;
	}

	for (int i = 0; i < queryBoxes.size(); i++) {
		cv::Scalar clr(dist(randE), dist(randE), dist(randE));
		cv::rectangle(queryImage, queryBoxes.at(i).box, clr, 2);
		cv::putText(queryImage, std::to_string(i), queryBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
	}
	utl::showImage("Query Image", queryImage);
	for (int i = 0; i < endBoxes.size(); i++) {
		cv::Scalar clr(dist(randE), dist(randE), dist(randE));
		cv::rectangle(endImage, endBoxes.at(i).box, clr, 2);
		cv::putText(endImage, std::to_string(i), endBoxes.at(i).box.tl(), cv::FONT_HERSHEY_COMPLEX, 1, clr, 2);
	}
	utl::showImage("End Image", endImage);
}
void ObjectMatcher::_calcHistogram(const cv::Mat &src, std::vector<double> &convCore, int convStep,
                                   std::vector<int> &featureVector) {
	cv::Mat hist;
	std::vector<cv::Mat> bgr_planes;
	cv::split(src, bgr_planes);

	int histSize = 256;
	int channels = 0;
	float ranges[] = { 0, 256 };
	const float *histRange[] = { ranges };

	featureVector.clear();

	for (auto & bgr_plane : bgr_planes) {
		cv::Mat planeHist;
		cv::calcHist(&bgr_plane, 1, &channels, cv::Mat(), planeHist, 1, &histSize, histRange);

		for (size_t winBegin = 0; winBegin + convCore.size() - 1 < planeHist.rows; winBegin += convStep) {
			double featureValue = 0;
			for (int idx = int(winBegin); idx < winBegin + convCore.size(); idx++) {
				featureValue += planeHist.at<uchar>(idx) * convCore.at(idx - winBegin);
			}
			featureVector.push_back(cvRound(featureValue));
		}
	}
}

void ObjectMatcher::_calcHistogram(const cv::Mat &src, cv::Mat &dstHist) {
	cv::Mat gray;

	if (src.type() == CV_8UC3) {
		cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
	} else {
		gray = src.clone();
	}

	int histSize = 256;
	int channels = 0;
	float ranges[] = { 0, 256 };
	const float *histRange[] = { ranges };

	cv::calcHist(&src, 1, &channels, cv::Mat(), dstHist, 1, &histSize, histRange);
}

void ObjectMatcher::_calcFeatureVectors(cv::Mat &src, std::vector<YOLOBox> &srcBoxes, std::vector<double> &convCore,
                                        int convStep, std::vector<Eigen::VectorXi> &featureVectors) {
	for (const YOLOBox &sb : srcBoxes) {
		std::vector<int> histVector;
		_calcHistogram(src(sb.box), convCore, convStep, histVector);

		histVector.push_back(sb.box.area());

		Eigen::VectorXi _featureVector = Eigen::VectorXi(histVector.size());
		std::copy(histVector.begin(), histVector.end(), _featureVector.data());
		featureVectors.push_back(_featureVector);

		/*
		std::stringstream ss;
		ss << "Hist.Conv \n" << "  Length: " << histVector.size() << "\n";
		Eigen::IOFormat iof(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[ ", " ]");
		ss << _featureVector.format(iof) << "\n";
		std::cout << ss.str();

		cv::Mat oImage;
		utl::drawBarChart(_featureVector, oImage);
		utl::showImage("FeatureVector.Conv", oImage);
		 */
	}
}

#endif